#include "connection.h"
#include "mavlink_channels.h"

///
/// \brief Connection::Connection
/// \param receiver_callback
///
Connection::Connection(receiver_callback_t receiver_callback) :
    _receiver_callback(receiver_callback),
    _mavlink_receiver()
{
    // Insert system ID 0 in all connections for broadcast.
    _system_ids.insert(0);
}

///
/// \brief Connection::~Connection
///
Connection::~Connection()
{
    // Just in case a specific connection didn't call it already.
    stop_mavlink_receiver();
    _receiver_callback = {};
}

///
/// \brief Connection::start_mavlink_receiver
/// \return
///
bool Connection::start_mavlink_receiver()
{
    uint8_t channel;
    if (!MAVLinkChannels::Instance().checkout_free_channel(channel)) {
        return false;
    }

    _mavlink_receiver.reset(new MAVLinkReceiver(channel));
    return true;
}

///
/// \brief Connection::stop_mavlink_receiver
///
void Connection::stop_mavlink_receiver()
{
    if (_mavlink_receiver) {
        uint8_t used_channel = _mavlink_receiver->get_channel();
        // Destroy receiver before giving the channel back.
        _mavlink_receiver.reset();
        MAVLinkChannels::Instance().checkin_used_channel(used_channel);
    }
}

///
/// \brief Connection::receive_message
/// \param message
/// \param connection
///
void Connection::receive_message(mavlink_message_t& message, Connection* connection)
{
    // Register system ID when receiving a message from a new system.
    if (_system_ids.find(message.sysid) == _system_ids.end()) {
        _system_ids.insert(message.sysid);
    }
    _receiver_callback(message, connection);
}

///
/// \brief Connection::has_system_id
/// \param system_id
/// \return
///
bool Connection::has_system_id(uint8_t system_id)
{
    return _system_ids.find(system_id) != _system_ids.end();
}

